using System;
using Microsoft.SPOT;
using Microsoft.SPOT.Hardware;
using System.Collections;
using System.Threading;
using Devantech.Hardware;

namespace MLRobotic.Asserv.Drivers
{
    public delegate void SetInt(int value);
    class SRF02
    {
        private static ArrayList lstCapteurs = new ArrayList();
        private static I2CDevice _device = null;
        private static int currentCapteur = 0;
        private static bool initing = true;

        DateTime dateLastAction = DateTime.MinValue;

        public ushort _address = 0;
        public int Distance;
        internal SRF02(ushort address)
        {
            try
            {
                _address = (ushort)(address >> 1);
                if (I2CSlave._slaveDevice == null)
                    I2CSlave._slaveDevice = new I2CDevice(new I2CDevice.Configuration(_address, 100));
                if (_device == null)
                    _device = I2CSlave._slaveDevice;
                
                lstCapteurs.Add(this);
            }
            catch
            {
                Debug.Print("Erreur SRF02 " + address.ToString());
            }
        }

        public void ChangeAddress(byte newAddress)
        {
            _device.Config = new I2CDevice.Configuration(_address, 100);
            I2CDevice.I2CTransaction[] xact = new I2CDevice.I2CTransaction[] {
                    I2CDevice.CreateWriteTransaction( new byte[] { 0x00, 0xA0 })
                };
            _device.Execute(xact, 500);
            Thread.Sleep(75);
            xact = new I2CDevice.I2CTransaction[] {
                    I2CDevice.CreateWriteTransaction( new byte[] { 0x00, 0xAA})
                };
            _device.Execute(xact, 500);
            Thread.Sleep(75);
            xact = new I2CDevice.I2CTransaction[] {
                    I2CDevice.CreateWriteTransaction( new byte[] { 0x00, 0xA5 })
                };
            _device.Execute(xact, 500);
            Thread.Sleep(75);
            xact = new I2CDevice.I2CTransaction[] {
                    I2CDevice.CreateWriteTransaction( new byte[] { 0x00, (byte) (newAddress) })
                };
            _device.Execute(xact, 500);
        }

        private bool Init()
        {
            if (DateTime.Now > dateLastAction.AddMilliseconds(50))
            {
                _device.Config = new I2CDevice.Configuration(_address, 100);
                // on passe en cm
                I2CDevice.I2CTransaction[] xact = new I2CDevice.I2CTransaction[] {
                    I2CDevice.CreateWriteTransaction( new byte[] { 0x00, 0x51 })
                };
                _device.Execute(xact, 500);
                dateLastAction = DateTime.Now;
                return true;
            }
            return false;
        }
        private bool GetValCm()
        {
            if (DateTime.Now > dateLastAction.AddMilliseconds(50))
            {
                _device.Config = new I2CDevice.Configuration(_address, 100);
                byte[] read = new byte[2];
                // on passe en cm
                I2CDevice.I2CTransaction[] xact = new I2CDevice.I2CTransaction[] {
                    I2CDevice.CreateWriteTransaction( new byte[] { 0x02 }),
                    I2CDevice.CreateReadTransaction( read)
                };
                _device.Execute(xact, 500);
                Distance = (read[1] | read[0] << 8);
                dateLastAction = DateTime.Now;
                return true;
            }
            return false;
        }

        public static void Process()
        {
            if (initing)
            {
                if (lstCapteurs.Count > 0)
                {
                    if (((SRF02)lstCapteurs[currentCapteur]).Init())
                    {
                        currentCapteur++;
                        if (currentCapteur == lstCapteurs.Count)
                        {
                            currentCapteur = 0;
                            initing = false;
                        }
                    }
                }

            }
            else if (((SRF02)lstCapteurs[currentCapteur]).GetValCm())
            {
                currentCapteur++;
                if (currentCapteur == lstCapteurs.Count)
                {
                    currentCapteur = 0;
                    initing = true;
                }
            }

            

        }

        //Exemple de code :
        //    us1 = new SRF02(0xF0);
        //    us2 = new SRF02(0xE0);

        //    while (true)
        //    {
        //        SRF02.Process();

        //        Debug.Print("Valeur : " + us1.Distance.ToString("F0") + " - " + us2.Distance.ToString("F0"));

        //        Thread.Sleep(200);
        //    }
    }
}
